#include <stdio.h>
#include <emmintrin.h>
#define ROWS_PROCESSED 20	// Each memory_vector processes 4 rows.  This corresponds to the number of VECTORS, NOT ROWS.  Update memory_vector array dimension.
#define COLS_PROCESSED 1	// TODO Remove; not necessary.
#define VECTOR_SIZE 4	// How many items can fit into a single 128-bit vector.

void sgemm( int m, int n, float *A, float *C ) {
	__m128 memory_vector[5], pivot_vector, column_vector, product_vector;
	int NUM_EXTRA_ROWS = m % VECTOR_SIZE, NUM_EXTRA_COLS = n % COLS_PROCESSED;
	float *memory_location, *col_offset_A, *pivot_offset_A;
	float pivot, *edge_location_A;

	for (int j = 0; j < m; j++) {	// TODO Outermost loop, general case.
		pivot_offset_A = A + j;

		for (int i = 0; i < m/ROWS_PROCESSED*ROWS_PROCESSED; i+=ROWS_PROCESSED) {	// TODO Middle loop, general case.
			memory_location = C + i + j*m;

			// Load memory_vectors from C into registers.
			*memory_vector = _mm_loadu_ps(memory_location);
			*(memory_vector+1) = _mm_loadu_ps(memory_location+4);
			*(memory_vector+2) = _mm_loadu_ps(memory_location+8);
			*(memory_vector+3) = _mm_loadu_ps(memory_location+12);
			*(memory_vector+4) = _mm_loadu_ps(memory_location+16);

			for (int k = 0; k < n / COLS_PROCESSED * COLS_PROCESSED; k+=COLS_PROCESSED) {	// TODO Inner loop, general case.
				// Constants for this loop: col_offset_A for selecting the column_vector, pivot_vector used.
				col_offset_A = A + i + k*m;
				pivot_vector = _mm_load1_ps(pivot_offset_A + k*m);

				column_vector = _mm_loadu_ps(col_offset_A);	// A + i + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*memory_vector = _mm_add_ps(*memory_vector, product_vector);

				column_vector = _mm_loadu_ps(col_offset_A+4);	// A + (i+4) + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*(memory_vector+1) = _mm_add_ps(*(memory_vector+1), product_vector);

				column_vector = _mm_loadu_ps(col_offset_A+8);	// A + (i+8) + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*(memory_vector+2) = _mm_add_ps(*(memory_vector+2), product_vector);

				column_vector = _mm_loadu_ps(col_offset_A+12);	// A + (i+12) + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*(memory_vector+3) = _mm_add_ps(*(memory_vector+3), product_vector);

				column_vector = _mm_loadu_ps(col_offset_A+16);	// A + (i+16) + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*(memory_vector+4) = _mm_add_ps(*(memory_vector+4), product_vector);

			}	// TODO End inner loop, general case.

			_mm_storeu_ps(memory_location, *memory_vector);
			_mm_storeu_ps(memory_location+4, *(memory_vector+1));
			_mm_storeu_ps(memory_location+8, *(memory_vector+2));
			_mm_storeu_ps(memory_location+12, *(memory_vector+3));
			_mm_storeu_ps(memory_location+16, *(memory_vector+4));

		}	// TODO End middle loop, general case.

		// Special case handling for rows.
		for (int i = m/ROWS_PROCESSED * ROWS_PROCESSED; i < m/VECTOR_SIZE * VECTOR_SIZE; i+= VECTOR_SIZE) {	// TODO Middle loop, columns special case.
			memory_location = C + i + j*m;
			*memory_vector = _mm_loadu_ps(memory_location);

			for (int k = 0; k < n / COLS_PROCESSED * COLS_PROCESSED; k+=COLS_PROCESSED) {	// TODO Inner loop, general case.
				// Constants for this loop: col_offset_A for selecting the column_vector, pivot_vector used.
				col_offset_A = A + i + k*m;
				pivot_vector = _mm_load1_ps(pivot_offset_A + k*m);

				column_vector = _mm_loadu_ps(col_offset_A);	// A + i + k*m
				product_vector = _mm_mul_ps(column_vector, pivot_vector);
				*memory_vector = _mm_add_ps(*memory_vector, product_vector);

			}	// TODO End inner loop, general case.
			_mm_storeu_ps(memory_location, *memory_vector);
		}	// TODO End middle loop, rows special case.

		switch (NUM_EXTRA_ROWS) {
		case 0:
			break;
		case 1:
			for (int k = 0; k < n; k++) {
				pivot = *(pivot_offset_A + k*m);
				edge_location_A = A + (m-NUM_EXTRA_ROWS) + k*m;
				*(C + (m-NUM_EXTRA_ROWS) + j*m) += pivot * *edge_location_A;
			}
			break;
		case 2:
			for (int k = 0; k < n; k++) {
				pivot = *(pivot_offset_A + k*m);
				edge_location_A = A + (m-NUM_EXTRA_ROWS) + k*m;
				*(C + (m-NUM_EXTRA_ROWS) + j*m) += pivot * *edge_location_A;
				*(C + (m-NUM_EXTRA_ROWS) + j*m + 1) += pivot * *(edge_location_A+1);
			}
			break;
		case 3:
			for (int k = 0; k < n; k++) {
				pivot = *(pivot_offset_A + k*m);
				edge_location_A = A + (m-NUM_EXTRA_ROWS) + k*m;
				*(C + (m-NUM_EXTRA_ROWS) + j*m) += pivot * *edge_location_A;
				*(C + (m-NUM_EXTRA_ROWS) + j*m + 1) += pivot * *(edge_location_A+1);
				*(C + (m-NUM_EXTRA_ROWS) + j*m + 2) += pivot * *(edge_location_A+2);
			}
			break;
		default:
			printf("*** If you see this message, something is wrong with how row special cases are handled.");
			break;
		}
	} // TODO End outer loop, general case.

}
